#include <GL/gl.h>

#include <cmath>
#include "vector.h"
#include "conePrimitive.h"
#include "defines/primitive.h"
#include "defines/axes.h"
#include "defines/rayState.h"

#define THETA 18

ConePrimitive::ConePrimitive(int _id) : Object(_id){
}

ConePrimitive::ConePrimitive(const ConePrimitive &_c, int _id)
    : Object(_c.getMaterial(), _c.getCenter(), _id, CONE, true, _c.getTransformation()), radius(_c.getRadius()), height(_c.getHeight()){
}

ConePrimitive::ConePrimitive(const Vector3D &_center, double _radius, double _height, const Material &_m, int _id)
    : Object(_m, _center, _id, CONE, true), radius(_radius), height(_height){
//    transformation.addTranslation(Vector3D(0, 0, _height));
//    transformation.addScale(Vector3D(0, 0, _height));
}

ConePrimitive::ConePrimitive(const Vector3D &_center, double _radius, double _height, const Material &_m, const Matrix &_matrix, const Matrix &_matrixI, int _id)
    : Object(_m, _center, _id, CONE, true, _matrix, _matrixI), radius(_radius), height(_height){
}

ConePrimitive::~ConePrimitive(){
}

double ConePrimitive::getRadius() const{
    return radius;
}

void ConePrimitive::setRadius(double _r){
    radius = _r;
}

double ConePrimitive::getHeight() const{
    return height;
}

void ConePrimitive::setHeight(double _h){
//    transformation.addTranslation(Vector3D(0, 0, _h - height));
    height = _h;
}

double ConePrimitive::volume() const{
    return (M_PI*radius*radius*height) / 3.0;
}

double ConePrimitive::area() const{
    return (M_PI*radius*radius) + (M_PI*radius*sqrt((radius*radius) + (height*height)));
}

void ConePrimitive::draw(bool _wireframe, bool _useParentColor, const Material &_parentColor) const{
    if(_wireframe){
//        glDisable(GL_LIGHT0);
//        glDisable(GL_LIGHTING);

        glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
        glDisable(GL_CULL_FACE);

//        glLineWidth(1);
    }

	glPushMatrix();
	glMultMatrixd(transformation.getLocalToWorld().toGlMatrix());

    Material color_ = chooseMaterial(_wireframe, _useParentColor, _parentColor);
    glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, color_.getAmbientGL());
    glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, color_.getDiffuseGL());
    glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, color_.getSpecularGL());

    glBegin(GL_TRIANGLE_FAN);
    glVertex3f(0, 0, 0);

    for(double i=0; i<=360.0; i = i+THETA){
        double theta = toRad(i);

        double x = radius*cos(theta);
        double y = radius*sin(theta);

        Vector3D normal = ( Vector3D(x, y, 0) ).normalize();
        glNormal3f(normal.x, normal.y, normal.z);

        glVertex3f(x, y, -height);
    }
    glEnd();

	//desenhando a base em baixo
    glBegin(GL_POLYGON);
    glNormal3f(0, 0, -1);

    for(double i=0; i<=360.0; i = i+THETA){
        double theta = toRad(-i);
        double x = radius*cos(theta);
        double y = radius*sin(theta);

        glVertex3f(x, y, -height);
    }
    glEnd();


	glPopMatrix();


    if(_wireframe){
//        glLineWidth(1);
//
//        glEnable(GL_LIGHT0);
//        glEnable(GL_LIGHTING);

        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
        glEnable(GL_CULL_FACE);
    }


    if(selected){
        Vector3D min, max;
        getMinMax(&min.x, &max.x, X_AXIS);
        getMinMax(&min.y, &max.y, Y_AXIS);
        getMinMax(&min.z, &max.z, Z_AXIS);

        drawBox(true, min - center, max - center);
    }
}

std::vector<Trio> ConePrimitive::intersection(const Ray &_ray) const{
    std::vector<Trio> result;

    Vector3D origin = _ray.getOrigin();
    //inserindo o primeiro ponto que TODOS terão
    result.push_back(Trio(origin, Vector3D(1,0,0), OUT));

    //colocnado o raio para coordenadas da primitiva
    Matrix toLocalVector = transformation.getWorldToLocal();
    Matrix toLocalDirection = transformation.getWorldToLocalDirection();

    origin = (toLocalVector * origin.toHomogeneousCoordinates()).fromHomogeneousCoordinates();
    Vector3D direction = Vector3D(toLocalDirection * Vector(_ray.getDirection()));


    //------------------------------------- interseção com o cilindro infinito
    double a = (direction.x * direction.x) + (direction.y * direction.y) - (direction.z * direction.z);
    double b = ((direction.x * origin.x) + (direction.y * origin.y) - (direction.z * origin.z)) * 2.0;
    double c = (origin.x * origin.x) + (origin.y * origin.y) - (origin.z * origin.z);
    //*** aqui poderia não precisar o raio ao quadrado se tivesse a trasnformação

    double delta = (b*b) - (4.0 * a * c);

//    std::cout << "\nValores\n\tA = " <<a <<"\n\tB = " << b << "\n\tC = " << c << "\n\tDelta = " <<delta<<std::endl;


    std::vector<Trio> possibleCases;

    if(fabs(direction.z) >= ERROR){
        double tdisc = (- height - origin.z) / direction.z;

        if(tdisc > -ERROR ){//é maior ou igual a zero
            Vector3D localP = origin + (direction * tdisc);

            //checar se o ponto passa dentro do circulo
            Vector3D pxy(localP.x, localP.y, 0);
            if(pxy.dotProduct(pxy) - ERROR <= radius * radius ){
//                std::cout << "inserido tdisc " << tdisc << std::endl;
                possibleCases.push_back(Trio(tdisc, localP, Vector3D(0,0,-1)));
            }
        }
    }

    if(fabs(delta) < ERROR){ //delta igual a zero
        if(fabs(a) >= ERROR){
            double t = - b / (2.0 * a);

    //        Matrix toWorld = transformation.getLocalToWorld();

            Vector3D localP = origin + (direction * t);
            Vector3D n = normal(localP);
//            std::cout << "a = " << a <<"\nLocal p: "<<std::endl;
//            localP.print();

            if( (localP.z + ERROR > -height) && (localP.z - ERROR < 0.0) ){
                if(t > -ERROR){ //se t == 0
    //                std::cout << "inserido t unico " << t << std::endl;
                    bool inserted = false;
                    for(unsigned int i = 0; i < possibleCases.size(); i++){
                        if(t < possibleCases.at(i).t){
    //                        Vector3D p = (toWorld * (localP).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                            possibleCases.insert(possibleCases.begin() + i, Trio(t, localP, n));

                            inserted = true;
                            break;
                        }
                    }
                    if(!inserted){
    //                    Vector3D p = (toWorld * (localP).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                        possibleCases.push_back(Trio(t, localP, n));
                    }
                }
            }
        }else{
            bool inserted = false;
            Vector3D localP(0,0,0);
            Vector3D n(0,0,1);
            double t = (localP - origin).length();
            for(unsigned int i = 0; i < possibleCases.size(); i++){
                if(t < possibleCases.at(i).t){
//                    Vector3D p = (toWorld * (localP).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                    possibleCases.insert(possibleCases.begin() + i, Trio(t, localP, n));

                    inserted = true;
                    break;
                }
            }
            if(!inserted){
//                    Vector3D p = (toWorld * (localP).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                possibleCases.push_back(Trio(t, localP, n));
            }
        }
    }else if(delta > 0.0){
        if(fabs(a) >= ERROR){
            double t0 = (- b - sqrt(delta)) / (2.0 * a);
            double t1 = (- b + sqrt(delta)) / (2.0 * a);

    //        Matrix toWorld = transformation.getLocalToWorld();

            Vector3D localP0 = origin + (direction * t0);
            Vector3D localP1 = origin + (direction * t1);
            Vector3D n0 = normal(localP0);
            Vector3D n1 = normal(localP1);

            unsigned int i = 0;
    //        std::cout << "t0 :" << t0 << "p0z = " << localP0.z<< std::endl;
            if( (localP0.z + ERROR > -height) && (localP0.z - ERROR < 0.0) ){// se tiver entre os z's
                if(t0 > - ERROR){//se for positivo
    //                std::cout << "inserido t0 " << t0 << std::endl;
                    bool inserted = false;
                    for(; i < possibleCases.size(); i++){
                        if(t0 < possibleCases.at(i).t){
    //                        Vector3D p0 = (toWorld * (localP0).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                            possibleCases.insert(possibleCases.begin() + i, Trio(t0, localP0, n0));
                            inserted = true;
                            i++;
                            break;
                        }
                    }
                    if(!inserted){
    //                    Vector3D p0 = (toWorld * (localP0).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                        possibleCases.push_back(Trio(t0, localP0, n0));
                        i++;
                    }
                }
            }

    //        std::cout << "t1 :" << t1 << "p1z = " << localP1.z<< std::endl;
            if( (localP1.z + ERROR > -height) && (localP1.z - ERROR < 0.0) ){
                if(t1 > - ERROR){//se for positivo
    //                std::cout << "inserido t1 " << t1 << std::endl;
                    bool inserted = false;
                    for(; i < possibleCases.size(); i++){
                        if(t1 < possibleCases.at(i).t){
    //                        Vector3D p1 = (toWorld * (localP1).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                            possibleCases.insert(possibleCases.begin() + i, Trio(t1, localP1, n1));

                            inserted = true;
                            i++;
                            break;
                        }
                    }
                    if(!inserted){
    //                    Vector3D p1 = (toWorld * (localP1).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                        possibleCases.push_back(Trio(t1, localP1, n1));
                    }
                }
            }
        }else{
            bool inserted = false;
            double t = (-c) / b;
//            std::cout <<"t = " << t <<std::endl;
            Vector3D localP = origin + (direction * t);
//            localP.print();
            Vector3D n = normal(localP);

            for(unsigned int i = 0; i < possibleCases.size(); i++){
                if(t < possibleCases.at(i).t){
//                    Vector3D p = (toWorld * (localP).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                    possibleCases.insert(possibleCases.begin() + i, Trio(t, localP, n));

                    inserted = true;
                    break;
                }
            }
            if(!inserted){
//                    Vector3D p = (toWorld * (localP).toHomogeneousCoordinates()).fromHomogeneousCoordinates();
                possibleCases.push_back(Trio(t, localP, n));
            }
        }
    }

//    std::cout <<"possibleCases.size() = " << possibleCases.size() <<std::endl;
//    for(unsigned int i = 0; i < possibleCases.size(); i++){
//        std::cout <<"ponto possivel " << i << ":\n\ta um t = " << possibleCases.at(i).t  <<std::endl;
//        possibleCases.at(i).point.print();
//    }

    Matrix toWorld = transformation.getLocalToWorld();
    Matrix toLocalTranpost = transformation.getWorldToLocalDirection().transpose();

    switch(possibleCases.size()){
        case 0:{
            break;
        }
        case 1:{//casos: na quina do cone ou começando de dentro

            Vector3D originXY(origin.x, origin.y, 0);
            if( ((origin.z + ERROR > -height ) && (origin.z < ERROR )) && (originXY.dotProduct(originXY) - ERROR <= radius_2(origin.z)) )
                result.back().type = IN;
            else
                result.push_back(Trio((toWorld * (possibleCases.front().point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                      Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize(), ON));

            if( fabs(possibleCases.front().t) < ERROR ){
                result.back().type = ON;
                result.back().normal = Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize();
            }

            result.push_back(Trio((toWorld * (possibleCases.front().point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                  Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize(), OUT));
            break;
        }
        case 2:{//casos: passando pelo meio(in), passando pela base(on), passando na ponta(on)
//            std::cout <<"Aqui!" <<std::endl;
            if( fabs(a) < ERROR ){
//                std::cout <<"-------1" <<std::endl;
                if( fabs(possibleCases.at(0).t) < ERROR ){
//                std::cout <<"-------1-2" <<std::endl;
                    result.back().type = ON;
                    result.back().normal = Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize();
                }
                Vector3D p1 = possibleCases.at(0).point;
                Vector3D p2 = possibleCases.at(1).point;
                if((fabs(p1.x) < ERROR && fabs(p1.y) < ERROR && fabs(p1.z) < ERROR) || (fabs(p2.x) < ERROR && fabs(p2.y) < ERROR && fabs(p2.z) < ERROR) )
                    result.push_back(Trio((toWorld * (possibleCases.at(0).point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                          Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize(), ON));
                else
                    result.push_back(Trio((toWorld * (possibleCases.at(0).point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                          Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize(), IN));
                result.push_back(Trio((toWorld * (possibleCases.at(1).point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                      Vector3D(toLocalTranpost * Vector(possibleCases.at(1).normal)).normalize(), OUT));
                break;
            }

//                std::cout <<"-------2" <<std::endl;

            Vector3D originXY(origin.x, origin.y, 0);
            if( ((origin.z + ERROR > -height ) && (origin.z < ERROR )) && (originXY.dotProduct(originXY) - ERROR <= radius_2(origin.z)) )
                result.back().type = IN;

            if( fabs(possibleCases.at(0).t) < ERROR ){
//                std::cout <<"-------3" <<std::endl;
                result.back().type = ON;
                result.back().normal = Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize();
            }
            result.push_back(Trio((toWorld * (possibleCases.at(0).point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                  Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize(), IN));

//                std::cout <<"(2) possibleCases.at(0).point.z - height = " << possibleCases.at(0).point.z - height << std::endl;
//                std::cout <<"(2) possibleCases.at(1).point.z - height = " << possibleCases.at(1).point.z - height << std::endl;
            if( fabs(possibleCases.at(0).t - possibleCases.at(1).t) < ERROR ){
//                std::cout <<"-------4" <<std::endl;
                result.back().type = ON;
                result.back().normal = Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize();
            }else if( (fabs(possibleCases.at(0).point.z + height) < ERROR) && (fabs(possibleCases.at(1).point.z + height) < ERROR) ) {
//                std::cout <<"(2) Caso on " << std::endl;
//                std::cout <<"-------5" <<std::endl;
                result.back().type = ON;
                result.back().normal = Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize();
            }
            result.push_back(Trio((toWorld * (possibleCases.at(1).point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                  Vector3D(toLocalTranpost * Vector(possibleCases.at(1).normal)).normalize(), OUT));


            break;
        }
        case 3:{
            if( fabs(possibleCases.at(0).t) < ERROR ){
                result.back().type = ON;
                result.back().normal = Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize();
            }
            result.push_back(Trio((toWorld * (possibleCases.at(0).point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                  Vector3D(toLocalTranpost * Vector(possibleCases.at(0).normal)).normalize(), IN));

//                    if( fabs(possibleCases.at(1).point - possibleCases.at(2).point) >= ERROR ){
//                        std::cout <<"Erro, tem 3 pontos mas os dois ultimos não são iguais" <<std::endl;
//                    }
            result.push_back(Trio((toWorld * (possibleCases.at(2).point).toHomogeneousCoordinates()).fromHomogeneousCoordinates(),
                                  Vector3D(toLocalTranpost * Vector(possibleCases.at(2).normal)).normalize(), OUT));

            break;
        }
    }

//    std::cout <<"result.size() = " << result.size() <<std::endl;
//    for(unsigned int i = 0; i < result.size(); i++){
//        std::cout <<"ponto possivel " << i << ":\n\ttipo" << result.at(i).type  <<std::endl;
//        result.at(i).point.print();
//    }

    return result;

}

void ConePrimitive::getMinMax(double *_min, double *_max, unsigned int _axis) const{
    switch(_axis){
        case X_AXIS:{
            *_min = center.x - radius;
            *_max = center.x + radius;
            break;
        }
        case Y_AXIS:{
            *_min = center.y - radius;
            *_max = center.y + radius;
            break;
        }
        case Z_AXIS:{
            *_min = center.z - height;
            *_max = center.z;
            break;
        }
    }
}

double ConePrimitive::radius_2(double _z) const{
    double zRadius = (radius * _z)/height;
    return zRadius*zRadius;
}

Vector3D ConePrimitive::normal(const Vector3D &_p) const{
    if(fabs(_p.x) < ERROR && fabs(_p.y) < ERROR && fabs(_p.z) < ERROR){
        return Vector3D(0, 0, 1);
    }
    Vector3D pCenter = _p;
    Vector3D generatrix = pCenter.normalize();
    pCenter.z = 0;
    pCenter = pCenter.normalize();

    Vector3D normal = (generatrix.crossProduct(pCenter)).crossProduct(generatrix);

    return normal.normalize();
}
